 #include <math.h>
 #include <time.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <ros/ros.h>
 #include <iostream> 
 #include <thread>
 #include <vector>
 #include <deque>
 #include <fstream>
 #include <string>

 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Float32.h>
 #include <std_msgs/Int64.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/PointCloud2.h>

 #include <tf/transform_datatypes.h>
 #include <tf/transform_broadcaster.h>

#include "lidar_gnss_mapping/odom_and_ctrl.h"
#include "lidar_gnss_mapping/odom_and_status.h"
#include "lidar_gnss_mapping/headinga_msg.h"
#include "math_process.hpp"

 #include <opencv/cv.h>
 #include <opencv2/highgui/highgui.hpp>

 #include <pcl_ros/point_cloud.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/ndt.h>
 #include <pcl/registration/ia_ransac.h>
 #include <pcl/filters/voxel_grid.h>
//法线估计
 #include <pcl/features/normal_3d.h>
 #include <pcl/features/fpfh.h>
 #include <pcl/search/kdtree.h>
#include <pcl/visualization/pcl_visualizer.h>

const double PI = 3.1415926;

const float scanPeriod = 0.1;

const int stackFrameNum = 1;
const int stackloamNum = 2;
int tmpstackloam = 0;
int tmpstackrtk = 0;
const int stackrtkNum = 2;
const int mapFrameNum = 5;

double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;

bool newLaserCloudCornerLast = false;
bool newLaserCloudSurfLast = false;
bool newLaserCloudFullRes = false;
bool newLaserOdometry = false;

int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;

int laserCloudValidInd[125];
int laserCloudSurroundInd[125];

pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerLast(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfLast(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerStack(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfStack(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerStack2(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfStack2(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudOri(new pcl::PointCloud<pcl::PointXYZI>());

pcl::PointCloud<pcl::PointXYZI>::Ptr coeffSel(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurround(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurround2(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudFullRes(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfArray[laserCloudNum];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerArray2[laserCloudNum];
pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfArray2[laserCloudNum];

pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<pcl::PointXYZI>());
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<pcl::PointXYZI>());

pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterCloud;


/////////////////////////////////////////////////
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>recentLidarCloud;    
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>originLidarCloud;    
std::vector<double>laser_time;

std::vector<std::vector<double>>rtk_transform;
std::vector<std::vector<double>>loam_transform;
std::vector<std::vector<double>>loam_transform_1;

std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>recentLidarCloudCorner;    
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>originLidarCloudCorner;   
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>recentLidarCloudSurf;
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>originLidarCloudSurf;   

////////////////////////////////////////////////
float transformSum[6] = {0};
float transformIncre[6] = {0};
float transformTobeMapped[6] = {0};
float transformBefMapped[6] = {0};
float transformAftMapped[6] = {0};

int imuPointerFront = 0;
int imuPointerLast = -1;
const int imuQueLength = 200;

double imuTime[imuQueLength] = {0};
float imuRoll[imuQueLength] = {0};
float imuPitch[imuQueLength] = {0};
////////////////////////////////////
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pcl::PointXYZI pointOri, pointSel, pointProj, coeff;
cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));
cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));

cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));

bool isDegenerate = false;
cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
int frameCount = stackFrameNum - 1;
int mapFrameCount = mapFrameNum - 1;

bool is_continue(false);
bool is_rtk(false);

nav_msgs::Odometry curGpsInsOdom;
double gitime;      
ROSPoi3d giposi;    
ROSVec3d girpy;     
int gistatus;       
int gisatnum;       
bool newGpsIns(false);  
double theta = 0,last_theta = 0.0;
double theta1 = 0,theta2 = 0,theta3 = 0;
double origin_x =0,origin_y = 0,origin_z = 0;
int last_status(0);
bool beginloam(false);
bool update(true);  
bool new_delta_theta(0);
double loam_delta_theta = 0.0;

Eigen::Matrix3f matrix_yaw = Eigen::Matrix3f::Identity();
Eigen::Matrix3f matrix_yaw_last = Eigen::Matrix3f::Identity();

long rtk_num = 0;
long rtk_index = 0;
ros::Publisher pubIndex;

float transformSum_odom[6] = {0};
float newloamodom[6]={0};
float lastloamodom[6]={0};
float newrtkodom[6]={0};
float first_loam_odom[6] = {0};
float resetloam[6] = {0};
ros::Publisher pubCleanOdom;

bool last_angle_status(true);
int last_rtk_status = 4;

bool isFirstAngle(true);
double first_yaw,first_pitch;
double angle_yaw,angle_pitch;
int last_publish_state = 0; 

 void transformAssociateToMap()
 {
   float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
            - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
   float y1 = transformBefMapped[4] - transformSum[4];
   float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
            + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);

   float x2 = x1;
   float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
   float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;

   transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
   transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
   transformIncre[5] = z2;
   //std::cout<<transformSum[0]<<","<<transformSum[1]<<","<<transformSum[2]<<","<<
   //transformSum[3]<<","<<transformSum[4]<<","<<transformSum[5]<<std::endl;
   float sbcx = sin(transformSum[0]);
   float cbcx = cos(transformSum[0]);
   float sbcy = sin(transformSum[1]);
   float cbcy = cos(transformSum[1]);
   float sbcz = sin(transformSum[2]);
   float cbcz = cos(transformSum[2]);

   float sblx = sin(transformBefMapped[0]);
   float cblx = cos(transformBefMapped[0]);
   float sbly = sin(transformBefMapped[1]);
   float cbly = cos(transformBefMapped[1]);
   float sblz = sin(transformBefMapped[2]);
   float cblz = cos(transformBefMapped[2]);

   float salx = sin(transformAftMapped[0]);
   float calx = cos(transformAftMapped[0]);
   float saly = sin(transformAftMapped[1]);
   float caly = cos(transformAftMapped[1]);
   float salz = sin(transformAftMapped[2]);
   float calz = cos(transformAftMapped[2]);

   float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly)
             - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
             - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
             - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
             - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
   transformTobeMapped[0] = -asin(srx);

   float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
                - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
                + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
   float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz)
                - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz)
                - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly)
                - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx)
                + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
   transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]),
                                  crycrx / cos(transformTobeMapped[0]));

   float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz)
                - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx)
                - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly)
                + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx)
                - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz
                + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz)
                + calx*cblx*salz*sblz);
   float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly)
                - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx)
                + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
                + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly)
                + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly
                - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz)
                - calx*calz*cblx*sblz);
   transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]),
                                  crzcrx / cos(transformTobeMapped[0]));

   x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];
   y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];
   z1 = transformIncre[5];

   x2 = x1;
   y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
   z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

   transformTobeMapped[3] = transformAftMapped[3]
                          - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);
   transformTobeMapped[4] = transformAftMapped[4] - y2;
   transformTobeMapped[5] = transformAftMapped[5]
                          - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
 }

 void transformUpdate()
 {
   if (imuPointerLast >= 0) {
     float imuRollLast = 0, imuPitchLast = 0;
     while (imuPointerFront != imuPointerLast) {
       if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) {
         break;
       }
       imuPointerFront = (imuPointerFront + 1) % imuQueLength;
     }

     if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) {
       imuRollLast = imuRoll[imuPointerFront];
       imuPitchLast = imuPitch[imuPointerFront];
     } else {
       int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
       float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack])
                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
       float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod)
                       / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);

       imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
       imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
     }

     transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
     transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;
   }

   //transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * transformSum[0];
   //transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * transformSum[2];

   for (int i = 0; i < 6; i++) {
     transformBefMapped[i] = transformSum[i];
     transformAftMapped[i] = transformTobeMapped[i];
   }
 }

 void pointAssociateToMap(pcl::PointXYZI *pi, pcl::PointXYZI *po)
 {
   float x1 = cos(transformTobeMapped[2]) * pi->x
            - sin(transformTobeMapped[2]) * pi->y;
   float y1 = sin(transformTobeMapped[2]) * pi->x
            + cos(transformTobeMapped[2]) * pi->y;
   float z1 = pi->z;

   float x2 = x1;
   float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;
   float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

   po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2
         + transformTobeMapped[3];
   po->y = y2 + transformTobeMapped[4];
   po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2
         + transformTobeMapped[5];
   po->intensity = pi->intensity;
 }

 void pointAssociateTobeMapped(pcl::PointXYZI *pi, pcl::PointXYZI *po)
 {
   float x1 = cos(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3])
            - sin(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]);
   float y1 = pi->y - transformTobeMapped[4];
   float z1 = sin(transformTobeMapped[1]) * (pi->x - transformTobeMapped[3])
            + cos(transformTobeMapped[1]) * (pi->z - transformTobeMapped[5]);

   float x2 = x1;
   float y2 = cos(transformTobeMapped[0]) * y1 + sin(transformTobeMapped[0]) * z1;
   float z2 = -sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;

   po->x = cos(transformTobeMapped[2]) * x2
         + sin(transformTobeMapped[2]) * y2;
   po->y = -sin(transformTobeMapped[2]) * x2
         + cos(transformTobeMapped[2]) * y2;
   po->z = z2;
   po->intensity = pi->intensity;
 }

 void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2)
 {
   timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec();

   laserCloudCornerLast->clear();
   pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);

   newLaserCloudCornerLast = true;
 }

 void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2)
 {
   timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec();

   laserCloudSurfLast->clear();
   pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);

   newLaserCloudSurfLast = true;
 }

 void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
 {
   timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();

   laserCloudFullRes->clear();
   pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);

   newLaserCloudFullRes = true;
 }

void ResetState() {
  std::memset(transformIncre, 0, sizeof(float) * 6);
  imuPointerFront = 0;
  imuPointerLast = -1;
  std::memset(transformTobeMapped, 0, sizeof(float) * 6);
  std::memset(transformBefMapped, 0, sizeof(float) * 6);
  std::memset(transformAftMapped, 0, sizeof(float) * 6);
  laserCloudCenWidth = 10;
  laserCloudCenHeight = 5;
  laserCloudCenDepth = 10;
  std::memset(laserCloudValidInd, 0, sizeof(int) * 125);
  std::memset(laserCloudSurroundInd, 0, sizeof(int) * 125);

  laserCloudCornerStack->clear();
  //laserCloudCornerStack->shrink_to_fit();
  laserCloudSurfStack->clear();
  laserCloudCornerStack2->clear();
  laserCloudSurfStack2->clear();
  laserCloudOri->clear();
  coeffSel->clear();
  laserCloudSurround->clear();
  laserCloudSurround2->clear();
  laserCloudCornerFromMap->clear();
  laserCloudSurfFromMap->clear();

  for (int i = 0; i < laserCloudNum; i++) {
    
    laserCloudCornerArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
    laserCloudSurfArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
    laserCloudCornerArray2[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
    laserCloudSurfArray2[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
    
  }
  pointSearchInd.clear();
  pointSearchSqDis.clear();

  std::memset(&pointOri, 0, sizeof(pcl::PointXYZI));
  std::memset(&pointSel, 0, sizeof(pcl::PointXYZI));
  std::memset(&pointProj, 0, sizeof(pcl::PointXYZI));
  std::memset(&coeff, 0, sizeof(pcl::PointXYZI));

  matA0 = cv::Mat(5, 3, CV_32F, cv::Scalar::all(0));
  matB0 = cv::Mat(5, 1, CV_32F, cv::Scalar::all(-1));
  matX0 = cv::Mat(3, 1, CV_32F, cv::Scalar::all(0));
  matA1 = cv::Mat(3, 3, CV_32F, cv::Scalar::all(0));
  matD1 = cv::Mat(1, 3, CV_32F, cv::Scalar::all(0));
  matV1 = cv::Mat(3, 3, CV_32F, cv::Scalar::all(0));

  isDegenerate = false;

  matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));

  frameCount = stackFrameNum - 1;
  mapFrameCount = mapFrameNum - 1;
}
void loamDeltaTheta(const nav_msgs::Odometry odom){
  new_delta_theta = true;
  
  resetloam[0] = odom.pose.pose.orientation.x;   //roll
  resetloam[1] = odom.pose.pose.orientation.y;   //pitch
  resetloam[2] = odom.pose.pose.orientation.z;   //yaw

  resetloam[3] = odom.pose.pose.position.x;    //x
  resetloam[4] = odom.pose.pose.position.y;    //y
  resetloam[5] = odom.pose.pose.position.z;    //z
  geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                       (resetloam[0], -resetloam[1], -resetloam[2]);

  nav_msgs::Odometry odomAftMapped;

  odomAftMapped.header.frame_id = "/camera_init";
  odomAftMapped.child_frame_id = "/aft_mapped";
  odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
  odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
  odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
  odomAftMapped.pose.pose.orientation.z = geoQuat.x;
  odomAftMapped.pose.pose.orientation.w = geoQuat.w;
  odomAftMapped.pose.pose.position.x = resetloam[3];
  odomAftMapped.pose.pose.position.y = resetloam[4];
  odomAftMapped.pose.pose.position.z = resetloam[5];
  pubCleanOdom.publish(odomAftMapped);
  transformSum[0] = resetloam[1];
  transformSum[1] = resetloam[2];
  transformSum[2] = resetloam[0];
  transformSum[3] = resetloam[3];
  transformSum[4] = resetloam[4];
  transformSum[5] = resetloam[5];


  loam_transform_1.clear();
  loam_transform.clear();

}
//angle data from dual-antenna GNSS
void HeadingCallback(const lidar_gnss_mapping::headinga_msg heading_angleIn){

    if(heading_angleIn.calculateStatus == ";SOL_COMPUTED"){
        if(isFirstAngle){
            isFirstAngle = false;
            first_yaw = 360 - heading_angleIn.yawAngle;
            first_pitch =  heading_angleIn.pitchAngle;
            angle_yaw = 0;
            angle_pitch = 0;
        }else{
            angle_yaw = (360 - heading_angleIn.yawAngle-first_yaw)*M_PI/180;
            angle_pitch = ( heading_angleIn.pitchAngle - first_pitch)*M_PI/180;
        }
        last_angle_status = true;
    }else
      last_angle_status = false;
}
 void GPSINSCallback(const lidar_gnss_mapping::odom_and_status msgIn) {

  gitime = msgIn.header.stamp.toSec();
  giposi = msgIn.odom.pose.pose.position;
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(msgIn.odom.pose.pose.orientation, orientation);
  tf::Matrix3x3(orientation).getRPY(girpy.x, girpy.y, girpy.z);

  ROSPoi3d tm_shift;
  Eigen::Vector3f normal_vec;
  normal_vec<<giposi.x,giposi.y,giposi.z;
  normal_vec = matrix_yaw*normal_vec;

  giposi.x = normal_vec[1];
  giposi.y = normal_vec[2] -0.27;
  giposi.z = normal_vec[0] + 1;

  ROSVec3d tm_rpy;
  tm_rpy.x = girpy.y;
  tm_rpy.y = girpy.z - theta; //Eliminating external factors
  tm_rpy.z = girpy.x;
  girpy = tm_rpy;

  last_rtk_status = msgIn.status;
  gistatus = msgIn.status;//GNSS state
  gisatnum = msgIn.sat_num;
  newGpsIns = true;
}
 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
 {
   timeLaserOdometry = laserOdometry->header.stamp.toSec();

   double roll, pitch, yaw;
   geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

   transformSum_odom[0] = -pitch;
   transformSum_odom[1] = -yaw;
   transformSum_odom[2] = roll;

   transformSum_odom[3] = laserOdometry->pose.pose.position.x;
   transformSum_odom[4] = laserOdometry->pose.pose.position.y;
   transformSum_odom[5] = laserOdometry->pose.pose.position.z;

   newLaserOdometry = true;
 }

 void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
 {
   double roll, pitch, yaw;
   tf::Quaternion orientation;
   tf::quaternionMsgToTF(imuIn->orientation, orientation);
   tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

   imuPointerLast = (imuPointerLast + 1) % imuQueLength;

   imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
   imuRoll[imuPointerLast] = roll;
   imuPitch[imuPointerLast] = pitch;
 }

int count = 0;
 void angCorrection(){

    ros::Rate rate(2);
    double sum_delta = 0.0;
    int tmp_index = 0;
    Eigen::Matrix4f transformation;
    std::vector<Eigen::Matrix4f>trans_Mat;
    std::vector<double>pitch_all;
    pcl::PointCloud<pcl::PointXYZI>::Ptr originlidar(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr recentlidar(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr recentlidar1(new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr recentlidar2(new pcl::PointCloud<pcl::PointXYZI>());
    
    int index = 0;
    int new_index=0;
    int old_index=0;
    double new_theta =0.0;
    double old_theta=0.0;
    bool first(true);
    Eigen::Matrix4f sac_trans; 
    Eigen::Affine3f correctionCameraFrame;
    float x, y, z, roll, pitch, yaw;
    int count = 1;
    int data_num;
    while(ros::ok()){

      if(update){
        new_index = loam_transform.size();
        old_index = first==true?3:3;  //10:3
        data_num = first==true? 40:20;

        if(update && (new_index-data_num)>old_index){
          count++;   
          originlidar.reset(new pcl::PointCloud<pcl::PointXYZI>());
          recentlidar.reset(new pcl::PointCloud<pcl::PointXYZI>());
          int tmp_i = 0;
          tmp_i = old_index;
          double delta_x = loam_transform[tmp_i][3] - loam_transform_1[tmp_i][3];
          double delta_y = loam_transform[tmp_i][4] - loam_transform_1[tmp_i][4];
          double delta_z = loam_transform[tmp_i][5] - loam_transform_1[tmp_i][5];
          double dist_delta = sqrt(delta_x*delta_x + delta_y*delta_y + delta_z*delta_z);
          double dist;
          double s = 1.0;

          for(int i = new_index - data_num;i<new_index;i++){
            pcl::PointXYZI rtk,loam;

            rtk.x = (loam_transform[i][5] - (loam_transform[(new_index - data_num)][5]));
            rtk.y = (loam_transform[i][3] - (loam_transform[(new_index - data_num)][3]));
            rtk.z = 0;
            loam.x = (loam_transform_1[i][5] - (loam_transform_1[(new_index - data_num)][5]));
            loam.y = (loam_transform_1[i][3] - (loam_transform_1[(new_index - data_num)][3]));
            loam.z = 0;

            dist = s*sqrt((rtk.x - loam.x)*(rtk.x - loam.x)+
                (rtk.x - loam.y)*(rtk.x - loam.y)+
                (rtk.x - loam.z)*(rtk.x - loam.z)); 
            recentlidar->push_back(loam);
            originlidar->push_back(rtk);
          }

          pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp_first;
          icp_first.setMaxCorrespondenceDistance(dist);
          icp_first.setMaximumIterations(500);
          icp_first.setTransformationEpsilon(1e-10);
          icp_first.setEuclideanFitnessEpsilon(0.01);
          icp_first.setRANSACIterations(0);
          icp_first.setInputSource(originlidar);
          icp_first.setInputTarget(recentlidar);
          pcl::PointCloud<pcl::PointXYZI>::Ptr unused_result(new pcl::PointCloud<pcl::PointXYZI>());
          icp_first.align(*unused_result);   
          sac_trans = icp_first.getFinalTransformation();
          correctionCameraFrame = icp_first.getFinalTransformation();
          pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);

          Eigen::Matrix3f tmp_matrix = Eigen::Matrix3f::Identity();
          tmp_matrix << cos(yaw), -sin(yaw), 0,
                        sin(yaw), cos(yaw), 0,
                        0        ,        0 , 1;
          std::cout<<"yaw "<<yaw<<std::endl;
          theta += yaw;
          rtk_index = rtk_num;
          loam_transform.clear();
          loam_transform_1.clear();
          update = false;
        }
      }
      rate.sleep();
    }
        
 }
 

lidar_gnss_mapping::odom_and_ctrl publishDataTypeConversion(std::vector<float>tmp){
  nav_msgs::Odometry odom;
  odom.header.frame_id = "/camera_init";
  odom.child_frame_id = "/aft_mapped";

  geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(
                                      tmp[2], -tmp[0], -tmp[1]);
  odom.header.stamp = ros::Time().fromSec(timeLaserOdometry);
  odom.pose.pose.orientation.x = -geoQuat.y;
  odom.pose.pose.orientation.y = -geoQuat.z;
  odom.pose.pose.orientation.z = geoQuat.x;
  odom.pose.pose.orientation.w = geoQuat.w;

  odom.pose.pose.position.x = tmp[3];
  odom.pose.pose.position.y = tmp[4];
  odom.pose.pose.position.z = tmp[5];
  lidar_gnss_mapping::odom_and_ctrl oc_msg1;
  oc_msg1.odom = odom;
  oc_msg1.ctrl_flag = tmp[6];
  return oc_msg1;
}
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "laserMapping");
   ros::NodeHandle nh;

   ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>
                                             ("/laser_cloud_corner_last", 50, laserCloudCornerLastHandler);

   ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>
                                           ("/laser_cloud_surf_last", 50, laserCloudSurfLastHandler);

   ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>
                                      ("/laser_odom_to_init", 100, laserOdometryHandler);

   ros::Subscriber subGpsInsOdomAndStatus = nh.subscribe<lidar_gnss_mapping::odom_and_status>
                                            ("/odom_and_status", 1000,GPSINSCallback);

  ros::Subscriber subHeading = nh.subscribe<lidar_gnss_mapping::headinga_msg>
                                    ("/heading/angle",1000,HeadingCallback);

   ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>
                                          ("/velodyne_cloud_3", 50, laserCloudFullResHandler);

   ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 500, imuHandler);
   ros::Subscriber subDeltaTheta = nh.subscribe<nav_msgs::Odometry>("/delta_theta",100,loamDeltaTheta);

   ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>
                                          ("/laser_cloud_surround", 1);

   ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
                                         ("/velodyne_cloud_registered", 5);
   ros::Publisher pubOdomAndRtk = nh.advertise<lidar_gnss_mapping::odom_and_ctrl>
                                  ("/laser_and_odom_init",5);

   ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init_2", 5);
   ros::Publisher pubOdomAndRtkRviz = nh.advertise<nav_msgs::Odometry>("/rviz_laser_and_rtk",10);
   pubCleanOdom = nh.advertise<nav_msgs::Odometry>("/clean_odom",10);
   nav_msgs::Odometry odomAftMapped;
   odomAftMapped.header.frame_id = "/camera_init";
   odomAftMapped.child_frame_id = "/aft_mapped";

   tf::TransformBroadcaster tfBroadcaster;
   tf::StampedTransform aftMappedTrans;
   aftMappedTrans.frame_id_ = "/camera_init";
   aftMappedTrans.child_frame_id_ = "/aft_mapped";

   pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterCorner;
   downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);

   downSizeFilterCloud.setLeafSize(0.01, 0.01, 0.01);

   pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterSurf;
   downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);

   pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterMap;
   downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);

   for (int i = 0; i < laserCloudNum; i++) {
     laserCloudCornerArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
     laserCloudSurfArray[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
     laserCloudCornerArray2[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
     laserCloudSurfArray2[i].reset(new pcl::PointCloud<pcl::PointXYZI>());
   }
   std::thread loopthread(angCorrection);

   int backCount = 0;
   ros::Rate rate(100);
   bool status = ros::ok();
   bool gps_right(false);
   bool loam_begin(false);
   while (status) {
     ros::spinOnce();

     if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
         fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
         fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
         fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {

        if(newLaserOdometry && newGpsIns){

            //if((gistatus == 4 || gistatus ==5)&&(last_status == 1 || last_status == 0|| last_status == 3) && gisatnum>=10){
            if((gistatus == 4 || gistatus ==5)&&(last_status == 1 || last_status == 0|| last_status == 3)){

                transformSum[0] = girpy.x;
                transformSum[1] = girpy.y;
                transformSum[2] = girpy.z;
                transformSum[3] = giposi.x;
                transformSum[4] = giposi.y;
                transformSum[5] = giposi.z;
                if(new_delta_theta){
                    new_delta_theta = false;
                    std::cout<<"reset"<<std::endl;
                    ResetState();
                    update = true;
                    loam_transform_1.clear();
                    loam_transform.clear();
                    geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                       (resetloam[0], -resetloam[1], -resetloam[2]);

                    odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                    odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
                    odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
                    odomAftMapped.pose.pose.orientation.z = geoQuat.x;
                    odomAftMapped.pose.pose.orientation.w = geoQuat.w;
                    odomAftMapped.pose.pose.position.x = resetloam[3];
                    odomAftMapped.pose.pose.position.y = resetloam[4];
                    odomAftMapped.pose.pose.position.z = resetloam[5];
                    pubCleanOdom.publish(odomAftMapped);
                    transformSum[0] = resetloam[0];
                    transformSum[1] = resetloam[1];
                    transformSum[2] = resetloam[2];   
                }
                gps_right = true;
                loam_begin = false;
            }
            else{
                update = false;
                if(gps_right && !loam_begin){
                    tmpstackloam = 0; 
                    ResetState();
                    geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                       (newrtkodom[2], -newrtkodom[0], -newrtkodom[1]); 
                    odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
                    odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
                    odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
                    odomAftMapped.pose.pose.orientation.z = geoQuat.x;
                    odomAftMapped.pose.pose.orientation.w = geoQuat.w;
                    odomAftMapped.pose.pose.position.x = newrtkodom[3];
                    odomAftMapped.pose.pose.position.y = newrtkodom[4];
                    odomAftMapped.pose.pose.position.z = newrtkodom[5];
                    pubCleanOdom.publish(odomAftMapped);
                    for(int i =0; i<6;++i){
                        newloamodom[i] = 0;
                        lastloamodom[i] = 0;
                    }
                }
                loam_begin = true;
                gps_right = false;
                for(int i = 0;i<6;++i){
                    transformSum[i] = transformSum_odom[i];
                }
            }
        }
       newLaserCloudCornerLast = false;
       newLaserCloudSurfLast = false;
       newLaserCloudFullRes = false;
       newLaserOdometry = false;

       frameCount++;
       if(frameCount >= stackFrameNum) {
         transformAssociateToMap();
         int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
         for (int i = 0; i < laserCloudCornerLastNum; i++) {
           pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
           laserCloudCornerStack2->push_back(pointSel);
         }

         int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
         for (int i = 0; i < laserCloudSurfLastNum; i++) {
           pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
           laserCloudSurfStack2->push_back(pointSel);
         }
       }

       if (frameCount >= stackFrameNum) {
         frameCount = 0;
         pcl::PointXYZI pointOnYAxis;
         pointOnYAxis.x = 0.0;
         pointOnYAxis.y = 10.0;
         pointOnYAxis.z = 0.0;
         pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);

         int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
         int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
         int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;

         if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
         if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
         if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;

         while (centerCubeI < 3) {
           for (int j = 0; j < laserCloudHeight; j++) {
             for (int k = 0; k < laserCloudDepth; k++) {
               int i = laserCloudWidth - 1;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; i >= 1; i--) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }
           centerCubeI++;
           laserCloudCenWidth++;
         }
         while (centerCubeI >= laserCloudWidth - 3) {
           for (int j = 0; j < laserCloudHeight; j++) {
             for (int k = 0; k < laserCloudDepth; k++) {
               int i = 0;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; i < laserCloudWidth - 1; i++) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }

           centerCubeI--;
           laserCloudCenWidth--;
         }

         while (centerCubeJ < 3) {
           for (int i = 0; i < laserCloudWidth; i++) {
             for (int k = 0; k < laserCloudDepth; k++) {
               int j = laserCloudHeight - 1;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; j >= 1; j--) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }

           centerCubeJ++;
           laserCloudCenHeight++;
         }

         while (centerCubeJ >= laserCloudHeight - 3) {
           for (int i = 0; i < laserCloudWidth; i++) {
             for (int k = 0; k < laserCloudDepth; k++) {
               int j = 0;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; j < laserCloudHeight - 1; j++) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }

           centerCubeJ--;
           laserCloudCenHeight--;
         }

         while (centerCubeK < 3) {
           for (int i = 0; i < laserCloudWidth; i++) {
             for (int j = 0; j < laserCloudHeight; j++) {
               int k = laserCloudDepth - 1;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; k >= 1; k--) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }

           centerCubeK++;
           laserCloudCenDepth++;
         }

         while (centerCubeK >= laserCloudDepth - 3) {
           for (int i = 0; i < laserCloudWidth; i++) {
             for (int j = 0; j < laserCloudHeight; j++) {
               int k = 0;
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeCornerPointer =
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCubeSurfPointer =
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
               for (; k < laserCloudDepth - 1; k++) {
                 laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)];
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                 laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];
               }
               laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeCornerPointer;
               laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
               laserCloudCubeSurfPointer;
               laserCloudCubeCornerPointer->clear();
               laserCloudCubeSurfPointer->clear();
             }
           }

           centerCubeK--;
           laserCloudCenDepth--;
         }
         int laserCloudValidNum = 0;
         int laserCloudSurroundNum = 0;
         for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {
           for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {
             for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {
               if (i >= 0 && i < laserCloudWidth &&
                   j >= 0 && j < laserCloudHeight &&
                   k >= 0 && k < laserCloudDepth) {

                 float centerX = 50.0 * (i - laserCloudCenWidth);
                 float centerY = 50.0 * (j - laserCloudCenHeight);
                 float centerZ = 50.0 * (k - laserCloudCenDepth);

                 bool isInLaserFOV = false;
                 for (int ii = -1; ii <= 1; ii += 2) {
                   for (int jj = -1; jj <= 1; jj += 2) {
                     for (int kk = -1; kk <= 1; kk += 2) {
                       float cornerX = centerX + 25.0 * ii;
                       float cornerY = centerY + 25.0 * jj;
                       float cornerZ = centerZ + 25.0 * kk;

                       float squaredSide1 = (transformTobeMapped[3] - cornerX)
                                          * (transformTobeMapped[3] - cornerX)
                                          + (transformTobeMapped[4] - cornerY)
                                          * (transformTobeMapped[4] - cornerY)
                                          + (transformTobeMapped[5] - cornerZ)
                                          * (transformTobeMapped[5] - cornerZ);

                       float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX)
                                          + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
                                          + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);

                       float check1 = 100.0 + squaredSide1 - squaredSide2
                                    - 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                       float check2 = 100.0 + squaredSide1 - squaredSide2
                                    + 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                       if (check1 < 0 && check2 > 0) {
                         isInLaserFOV = true;
                       }
                     }
                   }
                 }

                 if (isInLaserFOV) {
                   laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j
                                                        + laserCloudWidth * laserCloudHeight * k;
                   laserCloudValidNum++;
                 }
                 laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j
                                                              + laserCloudWidth * laserCloudHeight * k;
                 laserCloudSurroundNum++;
               }
             }
           }
         }

         laserCloudCornerFromMap->clear();
         laserCloudSurfFromMap->clear();
         for (int i = 0; i < laserCloudValidNum; i++) {
           *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
           *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
         }
         int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
         int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();

         int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();
         for (int i = 0; i < laserCloudCornerStackNum2; i++) {
           pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);
         }

         int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();
         for (int i = 0; i < laserCloudSurfStackNum2; i++) {
           pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);
         }

         laserCloudCornerStack->clear();
         downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
         downSizeFilterCorner.filter(*laserCloudCornerStack);
         int laserCloudCornerStackNum = laserCloudCornerStack->points.size();


         laserCloudSurfStack->clear();
         downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);
         downSizeFilterSurf.filter(*laserCloudSurfStack);
         int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

         laserCloudCornerStack2->clear();
         laserCloudSurfStack2->clear();
         if (laserCloudCornerFromMapNum > 100 && laserCloudSurfFromMapNum > 1000){
            kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
            kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
           for (int iterCount = 0; iterCount < 40; iterCount++) {
             laserCloudOri->clear();
             coeffSel->clear();

             for (int i = 0; i < laserCloudCornerStackNum; i++) {
               pointOri = laserCloudCornerStack->points[i];
               pointAssociateToMap(&pointOri, &pointSel);
               kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

               if (pointSearchSqDis[4] < 1.0) {
                 float cx = 0;
                 float cy = 0;
                 float cz = 0;
                 for (int j = 0; j < 5; j++) {
                   cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
                   cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
                   cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
                 }
                 cx /= 5;
                 cy /= 5;
                 cz /= 5;

                 float a11 = 0;
                 float a12 = 0;
                 float a13 = 0;
                 float a22 = 0;
                 float a23 = 0;
                 float a33 = 0;

                 for (int j = 0; j < 5; j++) {
                   float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
                   float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
                   float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;

                   a11 += ax * ax;
                   a12 += ax * ay;
                   a13 += ax * az;
                   a22 += ay * ay;
                   a23 += ay * az;
                   a33 += az * az;
                 }
                 a11 /= 5;
                 a12 /= 5;
                 a13 /= 5;
                 a22 /= 5;
                 a23 /= 5;
                 a33 /= 5;

                 matA1.at<float>(0, 0) = a11;
                 matA1.at<float>(0, 1) = a12;
                 matA1.at<float>(0, 2) = a13;
                 matA1.at<float>(1, 0) = a12;
                 matA1.at<float>(1, 1) = a22;
                 matA1.at<float>(1, 2) = a23;
                 matA1.at<float>(2, 0) = a13;
                 matA1.at<float>(2, 1) = a23;
                 matA1.at<float>(2, 2) = a33;

                 cv::eigen(matA1, matD1, matV1);
                 if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {

                   float x0 = pointSel.x;
                   float y0 = pointSel.y;
                   float z0 = pointSel.z;
                   float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                   float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                   float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                   float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                   float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                   float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                   float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                              * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                              + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                              * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                              + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
                              * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

                   float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                   float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                            + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

                   float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
                            - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                   float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
                            + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                   float ld2 = a012 / l12;

                   pointProj = pointSel;
                   pointProj.x -= la * ld2;
                   pointProj.y -= lb * ld2;
                   pointProj.z -= lc * ld2;

                   float s = 1 - 0.9 * fabs(ld2);

                   coeff.x = s * la;
                   coeff.y = s * lb;
                   coeff.z = s * lc;
                   coeff.intensity = s * ld2;

                   if (s > 0.1) {
                     laserCloudOri->push_back(pointOri);
                     coeffSel->push_back(coeff);
                   }
                 }
               }
             }
             for (int i = 0; i < laserCloudSurfStackNum; i++) {
               pointOri = laserCloudSurfStack->points[i];
               pointAssociateToMap(&pointOri, &pointSel);
               kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

               if (pointSearchSqDis[4] < 1.0) {
                 for (int j = 0; j < 5; j++) {
                   matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
                   matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
                   matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
                 }
                 cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);

                 float pa = matX0.at<float>(0, 0);
                 float pb = matX0.at<float>(1, 0);
                 float pc = matX0.at<float>(2, 0);
                 float pd = 1;

                 float ps = sqrt(pa * pa + pb * pb + pc * pc);
                 pa /= ps;
                 pb /= ps;
                 pc /= ps;
                 pd /= ps;

                 bool planeValid = true;
                 for (int j = 0; j < 5; j++) {
                   if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
                       pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
                       pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.2) {
                     planeValid = false;
                     break;
                   }
                 }

                 if (planeValid) {
                   float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                   pointProj = pointSel;
                   pointProj.x -= pa * pd2;
                   pointProj.y -= pb * pd2;
                   pointProj.z -= pc * pd2;

                   float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
                           + pointSel.y * pointSel.y + pointSel.z * pointSel.z));

                   coeff.x = s * pa;
                   coeff.y = s * pb;
                   coeff.z = s * pc;
                   coeff.intensity = s * pd2;

                   if (s > 0.1) {
                     laserCloudOri->push_back(pointOri);
                     coeffSel->push_back(coeff);
                   }
                 }
               }

             }
             
             float srx = sin(transformTobeMapped[0]);
             float crx = cos(transformTobeMapped[0]);
             float sry = sin(transformTobeMapped[1]);
             float cry = cos(transformTobeMapped[1]);
             float srz = sin(transformTobeMapped[2]);
             float crz = cos(transformTobeMapped[2]);

             int laserCloudSelNum = laserCloudOri->points.size();
             if (laserCloudSelNum < 50) {
               continue;
             }

              // bool tmp(false);

             if(!update && ((last_status == 0 || last_status ==1)))
             {
               cv::Mat matA(laserCloudSelNum, 3, CV_32F, cv::Scalar::all(0));
               cv::Mat matAt(3, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
               cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
               cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
               cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
               cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));

               for (int i = 0; i < laserCloudSelNum; i++) {
                 pointOri = laserCloudOri->points[i];
                 coeff = coeffSel->points[i];

                 float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                           + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                           + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

                 float ary = ((cry*srx*srz - crz*sry)*pointOri.x
                           + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                           + ((-cry*crz - srx*sry*srz)*pointOri.x
                           + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

                 float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                           + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                           + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;

                 matA.at<float>(i, 0) = arx;
                 matA.at<float>(i, 1) = ary;
                 matA.at<float>(i, 2) = arz;
                 matB.at<float>(i, 0) = -coeff.intensity;
               }//end for(int i=0;i<laserCloudSelNum;i++)
               cv::transpose(matA, matAt);

               matAtA = matAt * matA + 0.1*cv::Mat::eye(3,3,CV_32F);
               matAtB = matAt * matB;
               cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

               if (iterCount == 0) {              

                 cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
                 cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
                 cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));

                 cv::eigen(matAtA, matE, matV);
                 matV.copyTo(matV2);

                 isDegenerate = false;
                 float eignThre[3] = {100, 100, 100};
                 for (int i = 2; i >= 0; i--) {
                   if (matE.at<float>(0, i) < eignThre[i]) {
                     for (int j = 0; j < 3; j++) {
                       matV2.at<float>(i, j) = 0;
                     }
                     isDegenerate = true;
                   } else {
                     break;
                   }
                 }
                 matP = matV.inv() * matV2;
               }

               if (isDegenerate) {
                 cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
                 matX.copyTo(matX2);
                 matX = matP * matX2;
               }

               transformTobeMapped[0] += matX.at<float>(0, 0);
               transformTobeMapped[1] += matX.at<float>(1, 0);
               transformTobeMapped[2] += matX.at<float>(2, 0);
               transformTobeMapped[3] = giposi.x;
               transformTobeMapped[4] = giposi.y;
               transformTobeMapped[5] = giposi.z;

               float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
                            + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
                            + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
               if(deltaR < 0.005)
                  break;
             }
             else{
                 cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
                 cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
                 cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
                 cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
                 cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
                 cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
                 for (int i = 0; i < laserCloudSelNum; i++) {
                   pointOri = laserCloudOri->points[i];
                   coeff = coeffSel->points[i];

                   float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                             + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                             + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

                   float ary = ((cry*srx*srz - crz*sry)*pointOri.x
                             + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                             + ((-cry*crz - srx*sry*srz)*pointOri.x
                             + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

                   float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                             + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                             + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;

                   matA.at<float>(i, 0) = arx;
                   matA.at<float>(i, 1) = ary;
                   matA.at<float>(i, 2) = arz;
                   matA.at<float>(i, 3) = coeff.x;
                   matA.at<float>(i, 4) = coeff.y;
                   matA.at<float>(i, 5) = coeff.z;
                   matB.at<float>(i, 0) = -coeff.intensity;
                 }//end for(int i=0;i<laserCloudSelNum;i++)
                 cv::transpose(matA, matAt);
                 matAtA = matAt * matA + 0.1*cv::Mat::eye(6,6,CV_32F);
                 matAtB = matAt * matB;
                 cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
                 if (iterCount == 0) {
                    
                   cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
                   cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
                   cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
                   
                   cv::eigen(matAtA, matE, matV);
                   matV.copyTo(matV2);

                   isDegenerate = false;
                   float eignThre[6] = {100, 100, 100, 100, 100, 100};
                   for (int i = 5; i >= 0; i--) {//5
                     if (matE.at<float>(0, i) < eignThre[i]) {
                       for (int j = 0; j < 6; j++) {//6
                         matV2.at<float>(i, j) = 0;
                       }
                       isDegenerate = true;
                     } else {
                       break;
                     }
                   }
                   matP = matV.inv() * matV2;
                 }
                 if (isDegenerate) {
                   cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
                   matX.copyTo(matX2);
                   matX = matP * matX2;
                 }

                 transformTobeMapped[0] += matX.at<float>(0, 0);
                 transformTobeMapped[1] += matX.at<float>(1, 0);
                 transformTobeMapped[2] += matX.at<float>(2, 0);
                 transformTobeMapped[3] += matX.at<float>(3, 0);
                 transformTobeMapped[4] += matX.at<float>(4, 0);
                 transformTobeMapped[5] += matX.at<float>(5, 0);

                 float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
                              + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
                              + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
                 float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
                              + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
                              + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);

                 if(deltaR < 0.01 && deltaT < 0.02){
                    break;
                 }
             }

           }

           transformUpdate();
         }      
         rtk_num++;
         if((gistatus == 4 || gistatus == 5)){
            if(last_status == 3){
              if(tmpstackrtk < 3){
                is_rtk = true;
                tmpstackrtk++;
                rtk_num--;
              }else{
                is_rtk = false;
              }
              if(!is_rtk){
                last_status = 1;
                std::vector<float>data;
                for(int i = 0; i < 3; ++i){
                  data.push_back(transformAftMapped[i]);
                }
                data.push_back(giposi.x);
                data.push_back(giposi.y);
                data.push_back(giposi.z);
                data.push_back(1);      
                last_publish_state = 1;  
                lidar_gnss_mapping::odom_and_ctrl oc_msg1;
                oc_msg1 = publishDataTypeConversion(data);
                oc_msg1.rtk_num = rtk_index;
                data.clear();
                pubOdomAndRtk.publish(oc_msg1);
                pubOdomAndRtkRviz.publish(oc_msg1.odom);
                newrtkodom[0] = transformAftMapped[0];
                newrtkodom[1] = transformAftMapped[1];
                newrtkodom[2] = transformAftMapped[2];
                newrtkodom[3] = giposi.x;
                newrtkodom[4] = giposi.y;
                newrtkodom[5] = giposi.z;
              }
              else
                last_status = 3;

            }
            if(last_status == 1 || last_status == 0){
                last_status = 1;
                if(last_publish_state == 1){
                    last_publish_state = 0;
                    continue ;
                }
                std::vector<float>data;
                for(int i = 0; i < 3; ++i){
                  data.push_back(transformAftMapped[i]);
                }
                data.push_back(giposi.x);
                data.push_back(giposi.y);
                data.push_back(giposi.z);
                data.push_back(0);
                lidar_gnss_mapping::odom_and_ctrl oc_msg1;
                oc_msg1 = publishDataTypeConversion(data);
                data.clear();

                oc_msg1.theta = theta;
                oc_msg1.update = update;
                oc_msg1.rtk_num = rtk_index;
                pubOdomAndRtk.publish(oc_msg1);
                pubOdomAndRtkRviz.publish(oc_msg1.odom);

                newrtkodom[0] = transformAftMapped[0];
                newrtkodom[1] = transformAftMapped[1];
                newrtkodom[2] = transformAftMapped[2];
                newrtkodom[3] = giposi.x;
                newrtkodom[4] = giposi.y;
                newrtkodom[5] = giposi.z;
            }
            if(last_status ==2){

                for(int i=0; i<6; ++i){
                    lastloamodom[i] = newloamodom[i];
                    newloamodom[i] = transformAftMapped[i];
                }
                if(backCount<=35){
                    last_status = 2;
                    float deltaPose[6] = {0};

                    is_continue = false;
                    if(!is_continue){
                      std::cout<<"GPS is right delay *******************************"<<'\t'<<backCount<<std::endl;
                      backCount++;
                      for(int i = 0;i<6;++i){
                        deltaPose[i] = newloamodom[i] - lastloamodom[i];
                        lastloamodom[i] = newrtkodom[i];
                        newrtkodom[i] = newrtkodom[i] + deltaPose[i];   
                      }
                      std::vector<float>data;
                      for(int i = 0; i < 6; ++i){
                        data.push_back(newloamodom[i]);
                      }
                      data.push_back(2);     
                      last_publish_state = 2; 
                      lidar_gnss_mapping::odom_and_ctrl oc_msg1;
                      oc_msg1 = publishDataTypeConversion(data);
                      oc_msg1.update = update;
                      oc_msg1.rtk_num = rtk_index;
                      data.clear();

                      pubOdomAndRtk.publish(oc_msg1);  
                      pubOdomAndRtkRviz.publish(oc_msg1.odom);
                    }
                }
                else if(backCount>35){
                    last_status = 3;
                    backCount = 0;
                    tmpstackrtk = 0;
                    std::vector<float>data;
                    for(int i = 0; i < 6; ++i){
                      data.push_back(newloamodom[i]);
                    }
                    data.push_back(3);
                    last_publish_state = 3;
                    lidar_gnss_mapping::odom_and_ctrl oc_msg1;
                    oc_msg1 = publishDataTypeConversion(data);
                    oc_msg1.update = update;
                    oc_msg1.rtk_num = rtk_index;
                    data.clear();

                    pubOdomAndRtk.publish(oc_msg1); 
                    pubOdomAndRtkRviz.publish(oc_msg1.odom);
                }
            }
         }
         else{
            backCount = 0;
            for(int i=0; i<6; ++i){
                newloamodom[i] = transformAftMapped[i];
            }
            if(tmpstackloam<stackloamNum){
                is_continue = true;
                tmpstackloam++;    
                rtk_num--;
            }
            else
                is_continue = false;

            /****************************** */
            last_status = 2;
            float deltaPose[6] = {0};
            if(!is_continue){
                std::vector<float>data;
                for(int i = 0; i < 6; ++i){
                  data.push_back(newloamodom[i]);
                }
                data.push_back(2);    
                last_publish_state = 2;
                lidar_gnss_mapping::odom_and_ctrl oc_msg1;
                oc_msg1 = publishDataTypeConversion(data);
                oc_msg1.update = update;
                oc_msg1.rtk_num = rtk_index;
                data.clear();
                pubOdomAndRtk.publish(oc_msg1); 
                pubOdomAndRtkRviz.publish(oc_msg1.odom);
            }
         }
         pcl::PointXYZI pointTmp;
         for (int i = 0; i < laserCloudCornerLast->points.size(); i++) {
           pointAssociateToMap(&laserCloudCornerLast->points[i],&pointTmp);
         }
         for (int i = 0; i < laserCloudCornerStackNum; i++) {
           pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

           int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
           int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
           int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

           if (pointSel.x + 25.0 < 0) cubeI--;
           if (pointSel.y + 25.0 < 0) cubeJ--;
           if (pointSel.z + 25.0 < 0) cubeK--;

           if (cubeI >= 0 && cubeI < laserCloudWidth &&
               cubeJ >= 0 && cubeJ < laserCloudHeight &&
               cubeK >= 0 && cubeK < laserCloudDepth) {
             int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
             laserCloudCornerArray[cubeInd]->push_back(pointSel);
           }
         }
        std::vector<double>tmp_trans;
        tmp_trans.push_back(transformAftMapped[0]);
        tmp_trans.push_back(transformAftMapped[1]);
        tmp_trans.push_back(transformAftMapped[2]);
        tmp_trans.push_back(giposi.x);
        tmp_trans.push_back(giposi.y);
        tmp_trans.push_back(giposi.z);

        loam_transform.push_back(tmp_trans);
        tmp_trans.clear();
        tmp_trans.push_back(transformAftMapped[0]);
        tmp_trans.push_back(transformAftMapped[1]);
        tmp_trans.push_back(transformAftMapped[2]);
        tmp_trans.push_back(transformAftMapped[3]);
        tmp_trans.push_back(transformAftMapped[4]);
        tmp_trans.push_back(transformAftMapped[5]);
        loam_transform_1.push_back(tmp_trans);
        tmp_trans.clear();

         pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfStack_tmp(new pcl::PointCloud<pcl::PointXYZI>());
         laserCloudSurfStack_tmp->clear();
         for(int i = 0; i<laserCloudSurfLast->points.size(); ++i){
            pcl::PointXYZI pointTmp;
            pointAssociateToMap(&laserCloudSurfLast->points[i],&pointTmp);
            laserCloudSurfStack_tmp->push_back(pointTmp);
         }
         for (int i = 0; i < laserCloudSurfStackNum; i++) {
           pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
         //  laserCloudSurfStack_tmp->push_back(pointSel);

           int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
           int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
           int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

           if (pointSel.x + 25.0 < 0) cubeI--;
           if (pointSel.y + 25.0 < 0) cubeJ--;
           if (pointSel.z + 25.0 < 0) cubeK--;

           if (cubeI >= 0 && cubeI < laserCloudWidth &&
               cubeJ >= 0 && cubeJ < laserCloudHeight &&
               cubeK >= 0 && cubeK < laserCloudDepth) {
             int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
             laserCloudSurfArray[cubeInd]->push_back(pointSel);
           }
         }
        // std::cout<<"push_back laserCloudSurfLast  "<<laserCloudSurfLast->points.size()<<std::endl;
         pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfLast_tmp(new pcl::PointCloud<pcl::PointXYZI>());
         laserCloudSurfLast_tmp->clear();
         *laserCloudSurfLast_tmp += *laserCloudSurfLast;
         originLidarCloudSurf.push_back(laserCloudSurfLast_tmp);
         recentLidarCloudSurf.push_back(laserCloudSurfStack_tmp);


         for (int i = 0; i < laserCloudValidNum; i++) {
           int ind = laserCloudValidInd[i];

           laserCloudCornerArray2[ind]->clear();
           downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
           downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);

           laserCloudSurfArray2[ind]->clear();
           downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
           downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);

           pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudTemp = laserCloudCornerArray[ind];
           laserCloudCornerArray[ind] = laserCloudCornerArray2[ind];
           laserCloudCornerArray2[ind] = laserCloudTemp;

           laserCloudTemp = laserCloudSurfArray[ind];
           laserCloudSurfArray[ind] = laserCloudSurfArray2[ind];
           laserCloudSurfArray2[ind] = laserCloudTemp;
         }

         mapFrameCount++;
         if (mapFrameCount >= mapFrameNum) {
           mapFrameCount = 0;

           laserCloudSurround2->clear();
           for (int i = 0; i < laserCloudSurroundNum; i++) {
             int ind = laserCloudSurroundInd[i];
             *laserCloudSurround2 += *laserCloudCornerArray[ind];
             *laserCloudSurround2 += *laserCloudSurfArray[ind];
           }

           laserCloudSurround->clear();
           downSizeFilterCorner.setInputCloud(laserCloudSurround2);
           downSizeFilterCorner.filter(*laserCloudSurround);

           sensor_msgs::PointCloud2 laserCloudSurround3;
           pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
           laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
           laserCloudSurround3.header.frame_id = "/camera_init";
           pubLaserCloudSurround.publish(laserCloudSurround3);
         }

         int laserCloudFullResNum = laserCloudFullRes->points.size();
         if(laserCloudFullRes->points.size()!=0){
            originLidarCloud.push_back(laserCloudFullRes);     
         }
         
         for (int i = 0; i < laserCloudFullResNum; i++) {
           pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
         }
         if(laserCloudFullRes->points.size()!=0){
            recentLidarCloud.push_back(laserCloudFullRes);
         }


         sensor_msgs::PointCloud2 laserCloudFullRes3;
         pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
         laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
         laserCloudFullRes3.header.frame_id = "/camera_init";
         pubLaserCloudFullRes.publish(laserCloudFullRes3);

         geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                                   (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);

         odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
         odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
         odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
         odomAftMapped.pose.pose.orientation.z = geoQuat.x;
         odomAftMapped.pose.pose.orientation.w = geoQuat.w;
         odomAftMapped.pose.pose.position.x = transformAftMapped[3];
         odomAftMapped.pose.pose.position.y = transformAftMapped[4];
         odomAftMapped.pose.pose.position.z = transformAftMapped[5];
         odomAftMapped.twist.twist.angular.x = transformBefMapped[0];
         odomAftMapped.twist.twist.angular.y = transformBefMapped[1];
         odomAftMapped.twist.twist.angular.z = transformBefMapped[2];
         odomAftMapped.twist.twist.linear.x = transformBefMapped[3];
         odomAftMapped.twist.twist.linear.y = transformBefMapped[4];
         odomAftMapped.twist.twist.linear.z = transformBefMapped[5];
         pubOdomAftMapped.publish(odomAftMapped);

         aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);
         aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
         aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3],
                                              transformAftMapped[4], transformAftMapped[5]));
         tfBroadcaster.sendTransform(aftMappedTrans);
       }
     }
     status = ros::ok();
     rate.sleep();
   }
   loopthread.join();

   return 0;
 }